#include "HandleManager.h"
#include "C3D.h"
#include "modelerdraw.h"
#include <FL/gl.h>

HandleManager::HandleManager(void)
{
	readDataPoints();
	buildHandles();
}

HandleManager::~HandleManager(void)
{
}

void HandleManager::readDataPoints( void )
{
	if ( LoadC3DFile("walkRun.c3d", markers, &nFrame, &nMarker) ) {
		cout << "File has been read" << endl;
		cout << "Frames: " << nFrame << endl 
				<< "Markers: " << nMarker << endl
				<< "Total Data Points: " << markers.size() << endl
				<< "**************************" << endl;
	} else {
		cout << "File has NOT been read" << endl;
	}
}

void HandleManager::buildHandles( void )
{
	handles.push_back(Handle(LFHD,0.1016,0.1792,-0.0703, h_head) );
	handles.push_back(Handle(RFHD,0.1220,0.1750,0.0470, h_head) );
	handles.push_back(Handle(LBHD,-0.0649,0.1420,-0.0568, h_head) );
	handles.push_back(Handle(RBHD,-0.0564,0.1350,0.0621, h_head) );
	handles.push_back(Handle(C7,-0.1045,0.1013,-0.0088, h_spine) );
	handles.push_back(Handle(T10,-0.0699,0.0770,-0.0038, h_abdomen) );
	handles.push_back(Handle(CLAV,0.0960,0.1226,0.0071, h_spine) );
	handles.push_back(Handle(STRN,0.1431,0.1941,-0.0058, h_abdomen) );
	handles.push_back(Handle(RBAC,-0.1066,0.1351,-0.0032, h_scapula_right) );
	handles.push_back(Handle(LSHO,0.0022,0.2244,0.0356, h_scapula_left) );
	handles.push_back(Handle(LUPA,-0.0246,-0.1602,0.0162, h_bicep_left) );
	handles.push_back(Handle(LELB,-0.0006,-0.2633,-0.0272, h_bicep_left) );
	handles.push_back(Handle(LFRM,-0.0270,-0.1354,-0.0121, h_forearm_left) );
	handles.push_back(Handle(LWRA,0.0258,-0.2265,-0.0311, h_forearm_left) );
	handles.push_back(Handle(LWRB,-0.0440,-0.2151,-0.0011, h_forearm_left) );
	handles.push_back(Handle(LFIN,-0.0239,-0.1148,-0.0353, h_hand_left) );
	handles.push_back(Handle(RSHO,-0.0346,0.2194,-0.0498, h_scapula_right) );
	handles.push_back(Handle(RUPA,-0.0459,-0.1392,0.0229, h_bicep_right) );
	handles.push_back(Handle(RELB,-0.0022,-0.2701,0.0397, h_bicep_right) );
	handles.push_back(Handle(RFRM,-0.0138,-0.1455,0.0322, h_forearm_right) );
	handles.push_back(Handle(RWRA,0.0330,-0.2540,0.0395, h_forearm_right) );
	handles.push_back(Handle(RWRB,-0.0357,-0.2283,0.0297, h_forearm_right) );
	handles.push_back(Handle(RFIN,-0.0295,-0.1123,0.0707, h_hand_right) );
	handles.push_back(Handle(LFWT,0.0735,0.0704,-0.1545, h_root) );
	handles.push_back(Handle(RFWT,0.0957,0.0827,0.0654, h_root) );
	handles.push_back(Handle(LBWT,-0.0928,0.0520,-0.1075, h_root) );
	handles.push_back(Handle(RBWT,-0.0716,0.0699,0.0790, h_root) );
	handles.push_back(Handle(LTHI,0.0969,-0.1451,-0.0032, h_thigh_left) );
	handles.push_back(Handle(LKNE,-0.0159,-0.3867,-0.0395, h_thigh_left) );
	handles.push_back(Handle(LSHN,0.0115,-0.2288,-0.0026, h_shin_left) );
	handles.push_back(Handle(LANK,-0.0201,-0.3600,-0.0380, h_shin_left) );
	handles.push_back(Handle(LHEE,-0.0800,-0.04,-0.0179, h_foot_left) );
	handles.push_back(Handle(LTOE,0.1427,0.0040,0.0350, h_foot_left) );
	handles.push_back(Handle(LMT5,0.1492,0.0027,-0.0519, h_foot_left) );
	handles.push_back(Handle(RTHI,0.1018,-0.1285,0.0077, h_thigh_right) );
	handles.push_back(Handle(RKNE,-0.0064,-0.3716,0.0328, h_thigh_right) );
	handles.push_back(Handle(RSHN,0.0160,-0.1998,0.0144, h_shin_right) );
	handles.push_back(Handle(RANK,-0.0086,-0.3553,0.0258, h_shin_right) );
	handles.push_back(Handle(RHEE,-0.0737,-0.04,-0.0102, h_foot_right) );
	handles.push_back(Handle(RTOE,0.1635,-0.0077,-0.0189, h_foot_right) );
	handles.push_back(Handle(RMT5,0.1361,-0.0149,0.0634, h_foot_right) );
}

void HandleManager::drawMarkers( float t, float fps )
{
	int currentFrame = (t*fps);
	int start = currentFrame * nMarker;
	int end = start + nMarker;

	if(start > markers.size() || end > markers.size()) {
		//Take only last frame
		start = markers.size() - nMarker + 1;
		end = markers.size();
	}

	for(int i = start ; i < end; i++) {
		//cout << "Marker: " << i << endl;
		Vec3d marker = markers[i];
		glPointSize(1.0);
		glBegin(GL_POINTS);
		setDiffuseColor( 0.0, 0.0, 1.0 );
		glVertex3f(marker[0], marker[1], marker[2]);
		glEnd();
	}
	//cout << "-------" << endl;
}

Vec4d HandleManager::getMarker( float t, float fps, int pos )
{
	int currentFrame = (t*fps);
	int start = currentFrame * nMarker;
	int end = start + nMarker;

	if(start > markers.size() || end > markers.size()) {
		//Take only last frame
		start = markers.size() - nMarker + 1;
		end = markers.size();
	}
	//cout << "Marker: " << pos << " - " << start+pos << endl;
	Vec3d marker = markers[start + pos];
	Vec4d retVal;
	retVal[0] = marker[0];
	retVal[1] = marker[1];
	retVal[2] = marker[2];
	retVal[3] = 1;
	return retVal;
}

std::vector<Handle> HandleManager::getHandles( void )
{
	return handles;
}

void HandleManager::updateMarkers()
{

}